﻿#include <algorithm>
#include "JZGLGemo.h"

//JZPlane
QVector3D JZPlane::normal() const
{
    return QVector3D(A, B, C);
}

// 判断三点是否共线
bool arePointsCollinear(const QVector3D& a, const QVector3D& b, const QVector3D& c) {
    QVector3D ab = { b.x() - a.x(), b.y() - a.y(), b.z() - a.z() };
    QVector3D ac = { c.x() - a.x(), c.y() - a.y(), c.z() - a.z() };

    // 如果向量ab和ac对应分量成比例，则三点共线
    return (ab.x() * ac.y() == ab.y() * ac.x()) && (ab.x() * ac.z() == ab.z() * ac.x()) && (ab.y() * ac.z() == ab.z() * ac.y());
}

bool calculatePlane(const QVector3D& a, const QVector3D& b, const QVector3D& c, JZPlane* plane)
{
    if (arePointsCollinear(a, b, c))
        return false;

    float A, B, C, D;
    QVector3D ab = { b.x() - a.x(), b.y() - a.y(), b.z() - a.z() };
    QVector3D ac = { c.x() - a.x(), c.y() - a.y(), c.z() - a.z() };

    // 平面的法向量，通过向量叉乘得到
    A = ab.y() * ac.z() - ab.z() * ac.y();
    B = ab.z() * ac.x() - ab.x() * ac.z();
    C = ab.x() * ac.y() - ab.y() * ac.x();

    // 求法向量的模长
    float norm = std::sqrt(A * A + B * B + C * C);

    // 对法向量进行归一化
    A /= norm;
    B /= norm;
    C /= norm;

    // 求D的值，将点a代入平面方程 Ax + By + Cz + D = 0
    D = -(A * a.x() + B * a.y() + C * a.z());

    plane->A = A;
    plane->B = B;
    plane->C = C;
    plane->D = D;

    return true;
}

// 计算点到平面的距离
float distanceToPlane(const QVector3D& pt, const JZPlane& plane) {
    float x = pt.x();
    float y = pt.y();
    float z = pt.z();

    // 平面方程为 ax + by + cz + d = 0
    // 点 (x, y, z) 到平面的距离公式为 |ax + by + cz + d| / sqrt(a^2 + b^2 + c^2)
    float numerator = std::abs(plane.A * x + plane.B * y + plane.C * z + plane.D);
    float denominator = std::sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
    if (denominator == 0) {
        Q_ASSERT(0);
        return -1;
    }
    return numerator / denominator;
}

float calculatePlaneAngle(const JZPlane& plane1, const JZPlane& plane2)
{
    QVector3D normal1 = plane1.normal();
    QVector3D normal2 = plane2.normal();

    // 计算两个法向量的点积
    float dotProduct = QVector3D::dotProduct(normal1, normal2);

    // 计算两个法向量的模长
    float magnitude1 = normal1.length();
    float magnitude2 = normal2.length();

    // 计算夹角的余弦值
    float cosTheta = dotProduct / (magnitude1 * magnitude2);

    // 确保余弦值在[-1, 1]范围内，以避免由于浮点数精度问题导致的域错误
    cosTheta = std::max(-1.0f, std::min(1.0f, cosTheta));

    // 计算夹角（以弧度为单位）
    float angle = std::acos(cosTheta);
    float PI = 3.1415926;
    if (angle > PI / 2)
        angle = PI / 2 - angle;

    return angle;
}

// 计算点到平面的交点
bool lineIntersectionPlane(const JZLine& line,JZPlane &plane, QVector3D& inster) 
{
    QVector3D point = line.point;
    QVector3D direction = line.direction;

    double A = plane.A;
    double B = plane.B;
    double C = plane.C;
    double D = plane.D;

    // 计算分母 Am + Bn + Cp
    double denominator = A * direction.x() + B * direction.y() + C * direction.z();

    // 检查分母是否为零
    if (denominator == 0) {
        return false;
    }

    // 计算分子 -(Ax_0 + By_0 + Cz_0 + D)
    double numerator = -(A * point.x() + B * point.y() + C * point.z() + D);

    // 计算参数 t
    double t = numerator / denominator;

    // 计算交点坐标
    float x = point.x() + direction.x() * t;
    float y = point.y() + direction.y() * t;
    float z = point.z() + direction.z() * t;

    inster = QVector3D(x,y,z);
    return true;
}

bool perpendicularPlane(const QVector3D& p1, const QVector3D& p2, const JZPlane& plane, JZPlane& dist_plane) {
    // 计算过两点的向量
    QVector3D v = p2 - p1;

    // 给定平面的法向量
    QVector3D n1 = plane.normal();

    // 所求平面的法向量，通过叉积计算
    QVector3D n2 = QVector3D::crossProduct(n1,v);

    // 计算平面方程的常数项 D
    float D = -(n2.x() * p1.x() + n2.y() * p1.y() + n2.z() * p1.z());

    dist_plane = { n2.x(), n2.y(), n2.z(), D};
    return true;
}